#include <ros/ros.h>
#include <serial/serial.h>  //ROS已经内置了的串口包
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <px4flow/Flow.h>
#include <px4flow/Double_Odometry.h>
#include <nav_msgs/Odometry.h>
#include <string>
#include <string.h>
#include <sstream>
#include <iostream>
#include <fstream>
#include <vector>
#include <cmath>

// [need to install serial pkg first!!!!!] sudo apt-get install ros-kinetic-serial
px4flow::Flow miniflow_msg;
serial::Serial ser; //声明串口对象
float v_xy[2];//速度×高度数据为真实的速度，单位m/s
float angular_vel[3];
float yaw;
float hight;
float yaw_R[2][2];
float rv_xy[2];

#define scale_miniflow_f 1.0f

void data_printing(std::string &buffer){
    std::cout << "data:";
    std::string pkg_tmp;
    const int n = buffer.length();
    int i=0;
    for (i=0;i<n;++i){
        std::cout << "data:"<<std::endl;
        std::cout << (unsigned char)buffer[i];
    }
    std::cout<<"end;" <<std::endl;
}

// void camera_callback(const nav_msgs::Odometry& msg){
void camera_callback(const px4flow::Double_Odometry::ConstPtr& msg){

    angular_vel[0] = msg->twist_1.twist.angular.y;
    // angular_vel[0] = msg.twist.twist.angular.y;
    angular_vel[1] = -1.0f*msg->twist_1.twist.angular.x;
    // angular_vel[1] = -1.0f*msg.twist.twist.angular.x;
    // angular_vel[2] = msg.twist.twist.angular.z;
    angular_vel[2] = msg->twist_1.twist.angular.z;
}

void DistanceAcc_callback(const px4flow::Flow::ConstPtr& msg){
    yaw = msg->pos[2];
    // ROS_INFO("yaw: %2.3f",yaw);
    hight = msg->hight;
    yaw_R[0][0] = cos(yaw);
    yaw_R[0][1] = -sin(yaw);
    yaw_R[1][0] = sin(yaw);
    yaw_R[1][1] = cos(yaw);
}

int main (int argc, char** argv)
{
    //初始化节点
    ros::init(argc, argv, "miniflow_pub");
    //声明节点句柄
    ros::NodeHandle nh;

    //发布主题
    ros::Publisher miniflow_pub = nh.advertise<px4flow::Flow>("miniflow", 1);
    ros::Subscriber camera_sub = nh.subscribe("/double_camera/odom/sample", 1, camera_callback);
    // ros::Subscriber camera_sub = nh.subscribe("/camera_1/odom/sample", 1, camera_callback);
    ros::Subscriber DistanceAcc_sub = nh.subscribe("/DistanceAcc", 1, DistanceAcc_callback);
    try
    {
        //设置串口属性，并打开串口
        ser.setPort("/dev/ttyMiniflow");
        ser.setBaudrate(19200);
        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        ser.setTimeout(to);
        ser.open();
    }
    catch (serial::IOException& e)
    {
        ROS_ERROR_STREAM("Unable to open port ");
        return -1;
    }

    //检测串口是否已经打开，并给出提示信息
    if(ser.isOpen())
    {
        ROS_INFO_STREAM("Serial Port initialized");
    }
    else
    {
        return -1;
    }

    //指定循环的频率
    ros::Rate loop_rate(50);
    while(ros::ok())
    {

        if(ser.available()){
            // ROS_INFO_STREAM("Reading from serial port\n");
            // std::string result;
            // result = ser.read(ser.available());
            // // ROS_INFO_STREAM("Read: " << result.data);
            // std::cout << "data:"<<result<<std::endl;
            // read_pub.publish(result);
            std::string buffer;
            buffer = ser.read(ser.available());
            {
                int i = 0;
                std::string pkg_tmp;
                while (i < buffer.length()){
                    if ((unsigned char)buffer[i] == 0xFE && (unsigned char)buffer[i+1] == 0x04){
                        //check the pkg
                        pkg_tmp = buffer.substr(i+2,i+8);
                        int16_t tmp;
                        // int16_t time;//光流本次计算与上次计算的时间差
                        // double time = 0.0152;
                        uint8_t valid;
                        // time = int16_t((pkg_tmp[4]+pkg_tmp[5]*256)/1000);//单位ms
                        // time = (uint16_t)(pkg_tmp[5]<<8)|(pkg_tmp[4]);
                        // time = time /1000;//单位ms
                        for (int j=0; j<2; ++j){
                            tmp = (int16_t)(pkg_tmp[2*j+1]<<8)|(pkg_tmp[2*j]);
                            v_xy[j] = float(tmp)/(10.0f);
                        }
                        valid = pkg_tmp[5];
                        rv_xy[0] = yaw_R[0][0]*v_xy[0]+yaw_R[0][1]*v_xy[1];
                        rv_xy[1] = yaw_R[1][0]*v_xy[0]+yaw_R[1][1]*v_xy[1];
                        // ROS_INFO("v_x=%2.3f,v_y=%2.3f,state:[%1d]",rv_xy[0],rv_xy[1],(int)valid);
                        miniflow_msg.vx = (-1.0f*rv_xy[0]*scale_miniflow_f-angular_vel[0])*hight;
                        miniflow_msg.vy = (-1.0f*rv_xy[1]*scale_miniflow_f-angular_vel[1])*hight;
                        // miniflow_msg.vx = (-1.0f*rv_xy[0]*scale_miniflow_f)*hight;
                        // miniflow_msg.vy = (rv_xy[1]*scale_miniflow_f)*hight;
                        miniflow_msg.valid = (int)1;
                        miniflow_msg.quality = (int)valid;
                        miniflow_msg.header.stamp = ros::Time::now();
                        miniflow_msg.header.frame_id = "FRD";
                        miniflow_pub.publish(miniflow_msg);
                        i = i + 9;
                        break;
                    }else{
                        ++i;
                    }
                }
            }
        }
        ros::spinOnce();
        loop_rate.sleep();
    }
}
